/*
 * Copyright (c) 2011-2025, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/main/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#include "dart/collision/fcl/All.hpp"

#include <dart/All.hpp>

#include <gtest/gtest.h>
#if DART_HAVE_BULLET
  #include "dart/collision/bullet/All.hpp"
#endif
#include "../../helpers/GTestUtils.hpp"

using namespace dart;
using namespace collision;
using namespace dynamics;
using namespace dart::test;

namespace {

class DummyCollisionObject : public collision::CollisionObject
{
public:
  DummyCollisionObject(
      collision::CollisionDetector* detector, const dynamics::ShapeFrame* frame)
    : CollisionObject(detector, frame)
  {
  }

  void updateEngineData() override {}
};

} // namespace

//==============================================================================
TEST(Raycast, RayHitDefaultConstructor)
{
  RayHit rayHit;
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d::Zero().eval()));
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d::Zero().eval()));
  EXPECT_DOUBLE_EQ(rayHit.mFraction, 0);
}

//==============================================================================
TEST(Raycast, RaycastResultDefaultConstructor)
{
  RaycastResult result;
  EXPECT_FALSE(result.hasHit());
  EXPECT_TRUE(result.mRayHits.empty());
}

//==============================================================================
TEST(Raycast, RaycastOptionPassesFilterWhenUnset)
{
  auto detector = DARTCollisionDetector::create();

  auto frame = SimpleFrame::createShared(Frame::World());
  frame->setShape(std::make_shared<SphereShape>(0.1));
  DummyCollisionObject object(detector.get(), frame.get());

  RaycastOption option;

  EXPECT_FALSE(option.mEnableAllHits);
  EXPECT_FALSE(option.mSortByClosest);
  EXPECT_TRUE(option.passesFilter(&object));
}

//==============================================================================
TEST(Raycast, RaycastOptionHonorsPredicate)
{
  auto detector = DARTCollisionDetector::create();

  auto firstFrame = SimpleFrame::createShared(Frame::World());
  firstFrame->setShape(std::make_shared<SphereShape>(0.1));
  auto secondFrame = SimpleFrame::createShared(Frame::World());
  secondFrame->setShape(std::make_shared<SphereShape>(0.1));

  DummyCollisionObject first(detector.get(), firstFrame.get());
  DummyCollisionObject second(detector.get(), secondFrame.get());

  RaycastOption option(true, true, [&](const collision::CollisionObject* obj) {
    return obj == &first;
  });

  EXPECT_TRUE(option.mEnableAllHits);
  EXPECT_TRUE(option.mSortByClosest);
  EXPECT_TRUE(option.passesFilter(&first));
  EXPECT_FALSE(option.passesFilter(&second));
}

//==============================================================================
void testBasicInterface(const std::shared_ptr<CollisionDetector>& cd)
{
  if (cd->getType() != "bullet") {
    DART_WARN(
        "Aborting test: distance check is not supported by {}.", cd->getType());
    return;
  }

  auto simpleFrame1 = SimpleFrame::createShared(Frame::World());

  auto shape1 = std::make_shared<SphereShape>(1.0);
  simpleFrame1->setShape(shape1);

  auto group1 = cd->createCollisionGroup(simpleFrame1.get());

  EXPECT_EQ(group1->getNumShapeFrames(), 1u);

  collision::RaycastOption option;
  option.mEnableAllHits = false;

  collision::RaycastResult result;
  EXPECT_FALSE(result.hasHit());

  RayHit rayHit;

  result.clear();
  simpleFrame1->setTranslation(Eigen::Vector3d(5.0, 0.0, 0.0));
  cd->raycast(
      group1.get(),
      Eigen::Vector3d(-2, 0, 0),
      Eigen::Vector3d(2, 0, 0),
      option,
      &result);
  EXPECT_FALSE(result.hasHit());

  result.clear();
  simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
  cd->raycast(
      group1.get(),
      Eigen::Vector3d(-2, 0, 0),
      Eigen::Vector3d(2, 0, 0),
      option,
      &result);
  EXPECT_TRUE(result.hasHit());
  EXPECT_EQ(result.mRayHits.size(), 1u);
  rayHit = result.mRayHits[0];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.25);

  result.clear();
  simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
  simpleFrame1->setTranslation(Eigen::Vector3d(0.0, 0.0, 0.0));
  cd->raycast(
      group1.get(),
      Eigen::Vector3d(2, 0, 0),
      Eigen::Vector3d(-2, 0, 0),
      option,
      &result);
  EXPECT_TRUE(result.hasHit());
  EXPECT_EQ(result.mRayHits.size(), 1u);
  rayHit = result.mRayHits[0];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(1, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(1, 0, 0)));
  EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.25);

  result.clear();
  simpleFrame1->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0));
  cd->raycast(
      group1.get(),
      Eigen::Vector3d(-2, 0, 0),
      Eigen::Vector3d(2, 0, 0),
      option,
      &result);
  EXPECT_TRUE(result.hasHit());
  EXPECT_EQ(result.mRayHits.size(), 1u);
  rayHit = result.mRayHits[0];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(0, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_DOUBLE_EQ(rayHit.mFraction, 0.5);
}

//==============================================================================
TEST(Raycast, testBasicInterface)
{
  auto fcl = FCLCollisionDetector::create();
  testBasicInterface(fcl);

#if DART_HAVE_BULLET
  auto bullet = BulletCollisionDetector::create();
  testBasicInterface(bullet);
#endif

  auto dart = DARTCollisionDetector::create();
  testBasicInterface(dart);
}

//==============================================================================
void testOptions(const std::shared_ptr<CollisionDetector>& cd)
{
  if (cd->getType() != "bullet") {
    DART_WARN(
        "Aborting test: distance check is not supported by {}.", cd->getType());
    return;
  }

  auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
  auto shape1 = std::make_shared<SphereShape>(1.0);
  simpleFrame1->setShape(shape1);

  auto simpleFrame2 = SimpleFrame::createShared(Frame::World());
  auto shape2 = std::make_shared<SphereShape>(1.0);
  simpleFrame2->setShape(shape1);

  auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());

  collision::RaycastOption option;
  option.mEnableAllHits = false;

  collision::RaycastResult result;
  EXPECT_FALSE(result.hasHit());

  RayHit rayHit;

  result.clear();
  option.mEnableAllHits = false;
  option.mSortByClosest = false;
  simpleFrame1->setTranslation(Eigen::Vector3d(-2, 0, 0));
  simpleFrame2->setTranslation(Eigen::Vector3d(2, 0, 0));
  cd->raycast(
      group.get(),
      Eigen::Vector3d(-5, 0, 0),
      Eigen::Vector3d(5, 0, 0),
      option,
      &result);
  EXPECT_TRUE(result.hasHit());
  EXPECT_EQ(result.mRayHits.size(), 1u);
  rayHit = result.mRayHits[0];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-3, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_NEAR(rayHit.mFraction, 0.2, 1e-5);

  result.clear();
  option.mEnableAllHits = true;
  option.mSortByClosest = true;
  simpleFrame1->setTranslation(Eigen::Vector3d(-2, 0, 0));
  simpleFrame2->setTranslation(Eigen::Vector3d(2, 0, 0));
  cd->raycast(
      group.get(),
      Eigen::Vector3d(-5, 0, 0),
      Eigen::Vector3d(5, 0, 0),
      option,
      &result);
  EXPECT_TRUE(result.hasHit());
  EXPECT_EQ(result.mRayHits.size(), 2u);
  rayHit = result.mRayHits[0];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-3, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_NEAR(rayHit.mFraction, 0.2, 1e-5);
  rayHit = result.mRayHits[1];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(1, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_NEAR(rayHit.mFraction, 0.6, 1e-5);
}

//==============================================================================
TEST(Raycast, testOptions)
{
  auto fcl = FCLCollisionDetector::create();
  testOptions(fcl);

#if DART_HAVE_BULLET
  auto bullet = BulletCollisionDetector::create();
  testOptions(bullet);
#endif

  auto dart = DARTCollisionDetector::create();
  testOptions(dart);
}

//==============================================================================
void testFilters(const std::shared_ptr<CollisionDetector>& cd)
{
  if (cd->getType() != "bullet") {
    DART_WARN(
        "Aborting test: distance check is not supported by {}.", cd->getType());
    return;
  }

  auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
  auto simpleFrame2 = SimpleFrame::createShared(Frame::World());

  auto sphere = std::make_shared<SphereShape>(1.0);
  simpleFrame1->setShape(sphere);
  simpleFrame2->setShape(sphere);

  auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get());

  collision::RaycastOption option;
  collision::RaycastResult result;

  simpleFrame1->setTranslation(Eigen::Vector3d(-2, 0, 0));
  simpleFrame2->setTranslation(Eigen::Vector3d(2, 0, 0));

  option.mEnableAllHits = false;
  option.mSortByClosest = false;
  option.mFilter = [&](const collision::CollisionObject* obj) {
    return obj->getShapeFrame() == simpleFrame2.get();
  };

  result.clear();
  cd->raycast(
      group.get(),
      Eigen::Vector3d(-5, 0, 0),
      Eigen::Vector3d(5, 0, 0),
      option,
      &result);
  ASSERT_TRUE(result.hasHit());
  ASSERT_EQ(result.mRayHits.size(), 1u);
  auto rayHit = result.mRayHits[0];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(1, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_NEAR(rayHit.mFraction, 0.6, 1e-5);

  option.mEnableAllHits = true;
  option.mFilter = [&](const collision::CollisionObject* obj) {
    return obj->getShapeFrame() == simpleFrame1.get();
  };
  result.clear();
  cd->raycast(
      group.get(),
      Eigen::Vector3d(-5, 0, 0),
      Eigen::Vector3d(5, 0, 0),
      option,
      &result);
  ASSERT_TRUE(result.hasHit());
  ASSERT_EQ(result.mRayHits.size(), 1u);
  rayHit = result.mRayHits[0];
  EXPECT_TRUE(equals(rayHit.mPoint, Eigen::Vector3d(-3, 0, 0)));
  EXPECT_TRUE(equals(rayHit.mNormal, Eigen::Vector3d(-1, 0, 0)));
  EXPECT_NEAR(rayHit.mFraction, 0.2, 1e-5);

  option.mFilter = [&](const collision::CollisionObject*) {
    return false;
  };
  result.clear();
  cd->raycast(
      group.get(),
      Eigen::Vector3d(-5, 0, 0),
      Eigen::Vector3d(5, 0, 0),
      option,
      &result);
  EXPECT_FALSE(result.hasHit());
  EXPECT_TRUE(result.mRayHits.empty());
}

//==============================================================================
TEST(Raycast, testFilters)
{
#if HAVE_BULLET
  auto bullet = BulletCollisionDetector::create();
  testFilters(bullet);
#else
  GTEST_SKIP() << "Bullet collision detector not available.";
#endif
}
